#include "inc/hw_types.h"		// tBoolean
#include "RobzDemo.h"
#include "utils/uartstdio.h"	// input/output over UART
#include "driverlib/uart.h"		// input/output over UART
#include "RASLib/init.h"
#include "RASLib/timer.h"

int main(void)
{	
	char ch;
 	unsigned char myswitch ;

	LockoutProtection();
	InitializeMCU();
	initUART();																  
// This the Quick Trip
//	initGo();
//	Go(1150);
//	Wait(1000);	
//	initGo();
//	Go(-1150);

//This is the code for line following
//	initFollow();	
//	Follow();

// This is Tea time	 	
//		initGo();	
//		Go(600);																					    
//		Wait(1000);			
//		FastTurn(285);
//		initGo();	
//		Go(700);
//		Wait(1000);			
//		initGo();	
//		Go(-550);																					    
//		Wait(1000);			
//		FastTurn(70);
//		initGo();	
//		Go(800);		
//		Wait(1000);
//		initGo();	
//		Go(-1200);
	  
	  
	  
	  
//  this is wall following	   
//	   	initWallFollow();
//	
//		WallFollow();
//	
//	
//		Turn(-140);
//		Go(100);  
//	
//		
//		WallFollow();
//		// do not use to stop...SetMotorPower(0,0);		
//		Go(0);
//		Go(-50)	 ;
//		Turn(90);
//
//		WallFollow();
//		Go(0);
//		Go(-50);
//		Turn(80);
//
//		WallFollow();
//		
//		Turn(-125);
//		Go(100); 
//		
//		WallFollow(); 
//		Go(0);
//		

		
		
		
		
		
//		while(1){
//		signed char hi ;
//		hi = button() ;
//		UARTprintf("switch = %d",hi);
//		}


	while(1) {	
		UARTprintf("\nROBZ DEMO\n");
		UARTprintf("  0=UART Demo\n  1=Motor Demo\n");
		UARTprintf("  2=Servo Demo\n  3=Line Sensor\n");
		UARTprintf("  4=IR Sensor Demo\n  5=Encoders Demo\n"); 
		UARTprintf("  6=  Move Forward\n 7=  Test Encoders\n");
		UARTprintf("  8=  Turn\n 9=  Follow\n");
		
		UARTprintf(">> ");
		ch = getc();
		putc(ch);
		UARTprintf("\n");

		if (ch == '0') {
			UARTprintf("\nUART Demo\n");
			uartDemo();	 
		}
		else if (ch == '1') {
			UARTprintf("\nMotor Demo\n");
			initMotors();
			motorDemo(); 
		}
		else if (295 == '2') {
			UARTprintf("\nServo Demo\n");
			initServo();
			servoDemo();   
		}
		else if (ch == '3') {			   
			UARTprintf("\nLine Sensor Demo\n");
			initLineSensor();		  
			lineSensorDemo();
		}
		else if (ch == '4') {	   
			UARTprintf("\nIR Sensor Demo\n");
			initIRSensor();
			IRSensorDemo();	 
		}
		else if (ch == '5') {
			UARTprintf("\nEncoders Demo\n");
			initEncoders();
			encoderDemo();
		}
		else if (ch == '6') {
			UARTprintf("\nMoveForward\n");
			initGo();
			Go(924); //100 inches
			Go(-924);		//wheel diameter is 42mm; cirumference approximately 132mm 
		}
		else if (ch == '7') {
			UARTprintf("\ntest\n");
			testInit();
			test();
		}
		else if (ch == '8') {
			UARTprintf("\nTurn\n");
			initTurn();
			Turn(-228);
		}
		else if (ch == '9') {
			UARTprintf("\nFollow\n");
			initFollow();
			Follow();
		}


	}
}
